using System;
using System.Collections.Generic;
using System.Text;
using System.Threading;

using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

using Imagination.Partitioning;
using Imagination.Physics;
using System.Diagnostics;

namespace Imagination
{
    public class Thing
    {
		private static int			uniqueID = 0;
		
		private int				    id;
		protected bool				isLocked;
		
        protected Vector3			position; 
		protected Quaternion		orientation;
		public Matrix				World;
		public Vector3              Scale = new Vector3(1, 1, 1);


        protected Model				model;
		internal Zone				Zone;
        public BoundingSphere       Boundary;

        //  basic motion
        protected Vector3			velocity;
		protected Vector3			acceleration;
		protected Vector3			angularVelocity;
		protected Vector3			angularAcceleration;

        //  kinetic
		protected float				mass;
		protected float				invMass;
        protected List<Force>       forces;

        protected List<Constraint> constraints;

        //  rigid body
		protected Matrix			inertia;
		protected Matrix			invInertia;
		protected Vector3			moments;

        protected ImaginationEngine   engine;


        public List<Constraint> Constraints
        {
            get { return constraints; }
        }
        internal ImaginationEngine Engine
        {
            set { engine = value; }
        }

		public Matrix Inertia
		{
			get { return inertia; }
			set
			{
				inertia = value;
				invInertia = Matrix.Invert(value);
			}
		}
		
		public Matrix InverseInertia
		{
			get { return invInertia; }
			set
			{
				invInertia = value;
				inertia = Matrix.Invert(value);
			}
		}
		
		public Vector3 Moments
		{
			get { return moments; }
			set { moments = value; }
		}

		public float Mass
		{
			get { return mass; }
			set
			{
				mass = value;
				invMass = 1.0f / mass;
			}
		}
		
		public float InverseMass
		{
			get { return invMass; }
			set
			{
				invMass = value;
				mass = 1.0f / invMass;
			}
		}
						
		public List<Force> Forces
		{
			get { return forces; }
		}



		public Thing()
		{
			id = Interlocked.Increment(ref uniqueID);
			isLocked = false;

            position = Vector3.Zero;
            velocity = Vector3.Zero;
            acceleration = Vector3.Zero;

            orientation = Quaternion.Identity;
            angularVelocity = Vector3.Zero;
            angularAcceleration = Vector3.Zero;

            Mass = 1.0f;
            forces = new List<Force>();
            constraints = new List<Constraint>();

            Inertia = Matrix.Identity;
            moments = Vector3.Zero;

			model = null;
		}
		
		public bool IsLocked
		{
			get { return isLocked; }
			set { isLocked = value; }
		}
		
		public int ID
		{
			get { return id; }
		}
		
		public Vector3 Position
		{
			get { return position; }
			set { position = value; }
		}
		
		public Vector3 Velocity
		{
			get { return velocity; }
			set
            { 
                velocity = value;
                Debug.Assert(!float.IsNaN(velocity.X), this.ToString());
            }
		}
		
		public Vector3 Acceleration
		{
			get { return acceleration; }
			set { acceleration = value; }
		}
		
		public Quaternion Orientation
		{
			get { return orientation; }
			set { orientation = value; }
		}
		
		public Vector3 AngularVelocity
		{
			get { return angularVelocity; }
			set { angularVelocity = value; }
		}
		
		public Vector3 AngularAcceleration
		{
			get { return angularAcceleration; }
			set { angularAcceleration = value; }
		}
		
		public Model Model
		{
			get { return model; }
			set { model = value; }
		}

        public Vector3 Force
        {
            get
            {
                //  HACK: need to fix unit scale
                float unitScale = 1;// / PhysicalConstants.UnitScale;
                Vector3 totalForce = Vector3.Zero;
                foreach (Force force in forces)
                {
                    if (force.Value.HasValue)
                        totalForce += (force.Value.Value * unitScale);
                    else
                        totalForce += (force.Acceleration * unitScale * this.mass);
                }
                return totalForce;
            }
        }

        internal void ClearTemporaryForces()
        {
            for (int i = forces.Count - 1; i >= 0; i--)
            {
                Force force = forces[i];
                if (force.IsTemporary)
                    forces.Remove(force);
            }
        }

        public virtual void Update(GameTime gameTime)
        {
            float dt = (float)(gameTime.ElapsedGameTime.TotalSeconds);
            if (!isLocked)
            {
                velocity += acceleration * dt;
                position += velocity * dt;

                angularVelocity += angularAcceleration * dt;

                orientation = orientation * Quaternion.CreateFromAxisAngle(angularVelocity, angularVelocity.Length() * dt);
                orientation.Normalize();
                //orientation = orientation * Quaternion.RotationAxis(
                //    angularVelocity, angularVelocity.Length() * dt);
                
                //orientation.Normalize();
            }
            World = Matrix.CreateScale(Scale) *
                Matrix.CreateFromQuaternion(orientation) *
                Matrix.CreateTranslation(position);
        }

		public virtual void RespondToCollision(Imagination.Collisions.Collision c)
		{
            Thing t1 = c.Thing1;
            Thing t2 = c.Thing2;
            Vector3 pt1 = c.CollisionPoint - t1.Position;
            Vector3 pt2 = c.CollisionPoint - t2.Position;

            float massComponent = t1.InverseMass + t2.InverseMass;
            float inertia1 = Vector3.Dot(c.CollisionNormal, Vector3.Cross(
                Vector3.TransformNormal(Vector3.Cross(pt1, c.CollisionNormal),
                t1.InverseInertia), pt1));
            float inertia2 = Vector3.Dot(c.CollisionNormal, Vector3.Cross(
                Vector3.TransformNormal(Vector3.Cross(pt2, c.CollisionNormal),
                t2.InverseInertia), pt2));

            float impulseNumerator = -1.99f * c.RelativeVelocityN;
            Debug.Assert(!float.IsNaN(impulseNumerator) && !float.IsNegativeInfinity(impulseNumerator));

            float impulseDenominator = massComponent + inertia1 + inertia2;
            Debug.Assert(!float.IsNaN(impulseDenominator));
            float impulse = impulseNumerator / impulseDenominator;
            Debug.Assert(!float.IsNaN(impulse));

            float mu = 0.3f;
            Vector3 effImpulse = impulse * (c.CollisionNormal + mu * c.CollisionTangent);
            Debug.Assert(!float.IsNaN(effImpulse.X));

            if (this == t1)
            {
                t1.Velocity += effImpulse * t1.InverseMass;
                t1.AngularVelocity += Vector3.TransformNormal(
                    Vector3.Cross(effImpulse, pt1),
                    t1.InverseInertia);
            }
            else
            {
                t2.Velocity -= effImpulse * t2.InverseMass;
                t2.AngularVelocity -= Vector3.TransformNormal(
                    Vector3.Cross(effImpulse, pt2),
                    t2.InverseInertia);
            }
        }
		
		public virtual void Draw(GraphicsDevice device, Imagination.Display.Camera camera)
		{
			if (model != null)
			{
                Matrix[] transforms = new Matrix[model.Bones.Count];
                model.CopyAbsoluteBoneTransformsTo(transforms);

                //Draw the model, a model can have multiple meshes, so loop
                foreach (ModelMesh mesh in model.Meshes)
                {
                    //This is where the mesh orientation is set, as well as our camera and projection
                    foreach (BasicEffect effect in mesh.Effects)
                    {
                        effect.EnableDefaultLighting();
                        //effect.Texture = texs[texIndex];
                        effect.World = transforms[mesh.ParentBone.Index] * World;
                        effect.View = camera.View;
                        effect.Projection = camera.Projection;
                    }
                    //Draw the mesh, will use the effects set above.
                    mesh.Draw();
                }
            }
		}

    }
}
